/**
 * @file 	 VD130.c
 * @brief    VD130 型变频器驱动,当前驱动实现了多段速与通信给定的控制方式
 * @author 	 wenbo
 * @version  1.0
 * @date 	 2023-01-05
 *
 * @copyright Copyright (c) 2023  YGL
 *
 */

#include "vd130.h"
#include "stdlib.h"
#include "string.h"
#include "boardio.h"

 /*************************************************************************************/
static bool enLogOutput = false;
#define LOGI(fmt, ...)  do{                                                         \
                            if (enLogOutput)                                        \
                                println(fmt, ##__VA_ARGS__);                        \
                        }while(0)
#define LOGE(fmt, ...)  LOGI("ERROR >> " fmt, ##__VA_ARGS__)
#define CHECK_ERR(a, str, goto_tag, ...)                                   	        \
    do                                                                     	        \
    {                                                                      	        \
        if (!(a)) {                                                          	    \
            LOGE("%s(%d): " str, __FUNCTION__, __LINE__, ##__VA_ARGS__);		    \
            goto goto_tag;                                                 	        \
        }                                                                  	        \
    } while (0) 
/*************************************************************************************/
#define VD130_OFFLINE_THRESHOLD 3       // 离线阈值 (通信失败次数连续超过则判离线)
#define FAULT_LOOP_TIME_MS      3000    // 故障代码轮询时间

#define ADDR_CMD_SRC        0x0001      // P0-01 命令源选择
#define ADDR_MAX_FREQ       0x0002      // P0-02 最大频率
#define ADDR_A_FREQ_SRC     0x0005      // P0-05 主频率源 A 选择
#define ADDR_RUN_DIRECT     0x0009      // P0-09 运行方向
#define ADDR_ACC_TIME       0x000B      // P0-11 加速时间1
#define ADDR_DEC_TIME       0x000C      // P0-12 减速时间1
#define ADDR_BRAKE_TYPE     0x001E      // P0-30 停机方式
#define ADDR_BRAKE_FREQ     0x001F      // P0-31 停机直流制动起始频率
#define ADDR_BRAKE_CURR     0x0021      // P0-33 停机直流制动电流
#define ADDR_BRAKE_TIME     0x0022      // P0-34 停机直流制动时间
#define ADDR_ACC_TYPE       0x0025      // P0-37 加减速方式

#define ADDR_DI1_FUN        0x0201      // P2-01 DI1端子功能选择
#define ADDR_DI2_FUN        0x0202      // P2-02 DI2端子功能选择
#define ADDR_DI3_FUN        0x0203      // P2-03 DI3端子功能选择
#define ADDR_DI4_FUN        0x0204      // P2-04 DI4端子功能选择
#define ADDR_DI5_FUN        0x0205      // P2-05 DI5端子功能选择
#define ADDR_PORT_CMD_MODE  0x0207      // P2-07 端子命令方式
#define ADDR_AO_SEL         0x022D      // P2-45 AO 输出选择

#define ADDR_M_RATED_CURR   0x0309      // P3-09 电机额定电流
#define ADDR_M_RATED_VOL    0x030A      // P3-10 电机额定电压
#define ADDR_M_RATED_FREQ   0x030B      // P3-11 电机额定频率
#define ADDR_M_RATED_POWER  0x030C      // P3-12 电机额定功率
#define ADDR_M_RATED_SPEED  0x030D      // P3-13 电机额定转速

#define ADDR_STAGE_SPEED_0  0x0404      // 段速0
#define ADDR_STAGE_SPEED_1  0x0405      // 段速1
#define ADDR_STAGE_SPEED_2  0x0406      // 段速2
#define ADDR_STAGE_SPEED_3  0x0407      // 段速3
#define ADDR_STAGE_SPEED_4  0x0408      // 段速4
#define ADDR_STAGE_SPEED_5  0x0409      // 段速5
#define ADDR_STAGE_SPEED_6  0x040A      // 段速6
#define ADDR_STAGE_SPEED_7  0x040B      // 段速7
#define ADDR_STAGE_SPEED_8  0x040C      // 段速8

#define ADDR_COMM_OVER_TIME 0x0605      // 通信超时时间

#define ADDR_FAULT_CODE     0x1011      // 故障代码(只读)
#define ADDR_OUT_CURRENT    0x1016      // 输出电流(只读)

#define ADDR_COMM_SET_FREQ  0x1000      // 通讯设定频率值(只写)
#define ADDR_COMM_FUN_CTRL  0x1001      // 通讯功能控制(只写)


/**************************************************************************************************************/
static bool xp_vd130_switch_to_port_mode(VD130_CTX* const ctx);
static bool xp_vd130_switch_to_comm_mode(VD130_CTX* const ctx);
static void xp_vd130_write_pin(Type_GPIO_Def* const pIo, const bool isEnable);
static uint8_t xp_vd130_write_reginster(VD130_CTX* const ctx, uint16_t regAddr, uint16_t* pData, uint8_t regNum);
static uint8_t xp_vd130_read_reginster(VD130_CTX* const ctx, uint16_t regAddr, uint16_t* pData, uint8_t regNum);
static void vd130_polling_thread(void* arg);
/**************************************************************************************************************/



VD130_CTX* const xp_vd130_init(uint8_t slaveId, uint8_t port, uint32_t baudrate, uint8_t parity, vd130_event_callback event_cb)
{
    enLogOutput = true;
    VD130_CTX* const ctx = aos_calloc(1, sizeof(VD130_CTX));
    CHECK_ERR(ctx, "ctx calloc failed", err_1);
    CHECK_ERR(0 == xp_modbus_init(port, baudrate, parity, 100), "vd130 modbus init failed", err_2);

    *ctx = VD130_DEFAULT_PARAMS();

    ctx->slaveId = slaveId;
    ctx->port = port;
    ctx->isOnline = false;
    ctx->lostCnts = 0;

    /* 不注册回调函数将不会创建轮询线程,如果使用通信给定方式,则需要应用层维护超时心跳 */
    if (event_cb) {
        xp_vd130_set_loop_state(ctx, false);    // 默认不轮询读取电流值
        ctx->event_cb = event_cb;
        /* 轮询间隔默认设置为通信超时时间的 1/3 */
        xp_vd130_set_thread_loop_interval_ms(ctx, ctx->commOvertime_ms / 3);
        char threadName[30];
        sprintf(threadName, "vd130_polling_thread_%d", slaveId);
        aos_task_new(threadName, vd130_polling_thread, ctx, 4096);
    }
    aos_mutex_new(ctx->mutex);
    // enLogOutput = false;    // 默认关闭 LOG

    return ctx;
err_2:
    aos_free(ctx);
err_1:
    enLogOutput = true;
    return NULL;
}
bool xp_vd130_deinit(VD130_CTX* ctx)
{
    CHECK_ERR(ctx, "ctx null", err_1);
    if (ctx->event_cb) {
        char threadName[30];
        sprintf(threadName, "vd130_polling_thread_%d", ctx->slaveId);
        aos_task_delete(threadName);
    }
    aos_mutex_free(ctx->mutex);
    aos_free(ctx);
    ctx = NULL;
    return true;
err_1:
    return false;
}
bool xp_vd130_set_loop_state(VD130_CTX* const ctx, bool isReadCurrent)
{
    CHECK_ERR(ctx, "ctx null", err_1);
    CHECK_ERR(ctx->event_cb, "event_cb not exist", err_1);
    ctx->isReadCurrent = isReadCurrent;
    if (VD130_MODE_COMM == ctx->mode) {     // 配置停止电流读取时,关闭通信超时检测,否则可能触发通信超时故障报警
        uint16_t u16Data = isReadCurrent ? (ctx->commOvertime_ms / 100) : (0);
        CHECK_ERR(xp_vd130_write_reginster(ctx, ADDR_COMM_OVER_TIME, &u16Data, 1), "ADDR_COMM_OVER_TIME failed", err_1);
    }
    return true;
err_1:
    return false;
}
void xp_vd130_set_thread_loop_interval_ms(VD130_CTX* const ctx, uint32_t ms)
{
    CHECK_ERR(ctx, "ctx null", err_1);
    ctx->interval_ms = (ms < 10) ? (10) : (ms);
err_1:
    return;
}
bool xp_vd130_is_online(VD130_CTX* const ctx)
{
    CHECK_ERR(ctx, "ctx null", err_1);
    return ctx->lostCnts > VD130_OFFLINE_THRESHOLD;
err_1:
    return false;
}
uint8_t xp_vd130_get_id(VD130_CTX* const ctx)
{
    CHECK_ERR(ctx, "ctx null", err_1);
    return ctx->slaveId;
err_1:
    return 0;
}
bool xp_vd130_stageSpeed_config(VD130_CTX* const ctx, int16_t stageSpeedList[], uint8_t listLen)
{
    CHECK_ERR(ctx, "ctx null", err_1);
    for (size_t i = 0; i < listLen && i < sizeof(ctx->stageSpeedList) / sizeof(ctx->stageSpeedList[0]); i++) {
        // 多段速 x -P0-02～P0-02
        uint16_t u16Data = ctx->stageSpeedList[i] = stageSpeedList[i];
        CHECK_ERR(1 == xp_vd130_write_reginster(ctx, ADDR_STAGE_SPEED_0 + i, &u16Data, 1), "ADDR_STAGE_SPEED_x failed", err_1);
    }
    return true;
err_1:
    return false;
}
bool xp_vd130_DI_func_config(VD130_CTX* const ctx, int16_t DI_func[], uint8_t listLen)
{
    CHECK_ERR(ctx, "ctx null", err_1);
    for (size_t i = 0; i < listLen && i < sizeof(ctx->DIx) / sizeof(ctx->DIx[0]); i++) {
        // DIx 端子功能选择
        uint16_t u16Data = ctx->DIx[i].func = DI_func[i];
        CHECK_ERR(1 == xp_vd130_write_reginster(ctx, ADDR_DI1_FUN + i, &u16Data, 1), "ADDR_DI1_FUN failed", err_1);
    }
    return true;
err_1:
    return false;
}
bool xp_vd130_run_stageSpeed(VD130_CTX* const ctx, uint8_t stageIndex)
{
    CHECK_ERR(ctx, "ctx null", err_1);
    if (VD130_MODE_PORT != ctx->mode) {
        CHECK_ERR(xp_vd130_switch_to_port_mode(ctx), "xp_vd130_switch_to_port_mode failed", err_1);
    }
    uint8_t index = 0;
    for (size_t i = 0; i < sizeof(ctx->DIx) / sizeof(ctx->DIx[0]); i++) {
        {
            if (ctx->DIx[i].func >= 13 && ctx->DIx[i].func <= 16) {
                xp_vd130_write_pin(&ctx->DIx[i].io, (stageIndex & (1 << index++)) ? (true) : (false));
            }
        }
    }
    return true;
err_1:
    return false;
}
bool xp_vd130_run_comm(VD130_CTX* const ctx, float freq)
{
    CHECK_ERR(ctx, "ctx null", err_1);
    if (VD130_MODE_COMM != ctx->mode) {
        CHECK_ERR(xp_vd130_switch_to_comm_mode(ctx), "xp_vd130_switch_to_comm_mode failed", err_1);
    }

    uint16_t u16Data = freq * 10;
    CHECK_ERR(1 == xp_vd130_write_reginster(ctx, ADDR_COMM_SET_FREQ, &u16Data, 1), "ADDR_COMM_SET_FREQ failed", err_1);

    return true;
err_1:
    return false;
}
bool xp_vd130_get_current(VD130_CTX* const ctx, uint16_t* pResult)
{
    CHECK_ERR(ctx, "ctx null", err_1);
    CHECK_ERR(1 == xp_vd130_read_reginster(ctx, ADDR_OUT_CURRENT, &ctx->current, 1), "ADDR_OUT_CURRENT failed", err_1);
    if (pResult) {
        *pResult = ctx->current;
    }
    return true;
err_1:
    return false;
}
bool xp_vd130_get_fault_code(VD130_CTX* const ctx, uint16_t* pResult)
{
    uint16_t u16Data;

    CHECK_ERR(ctx, "ctx null", err_1);
    CHECK_ERR(1 == xp_vd130_read_reginster(ctx, ADDR_FAULT_CODE, &u16Data, 1), "ADDR_FAULT_CODE failed", err_1);
    if (pResult) {
        *pResult = u16Data;
    }
    return true;
err_1:
    return false;
}
bool xp_vd130_clear_fault(VD130_CTX* const ctx)
{
    CHECK_ERR(ctx, "ctx null", err_1);
    bool isCfgClearFaultIO = false;
    if (VD130_MODE_PORT == ctx->mode) {
        for (size_t i = 0; i < sizeof(ctx->DIx) / sizeof(ctx->DIx[0]); i++) {
            if (9 == ctx->DIx[i].func) {     // 遍历IO功能配置列表,如果配置了清除故障IO,则通过IO清除故障
                isCfgClearFaultIO = true;
                xp_vd130_write_pin(&ctx->DIx[i].io, true);
                aos_msleep(100);
                xp_vd130_write_pin(&ctx->DIx[i].io, false);
                break;
            }
        }
    }
    if (VD130_MODE_COMM == ctx->mode || !isCfgClearFaultIO) {
        uint16_t u16Data = 7;
        CHECK_ERR(1 == xp_vd130_write_reginster(ctx, ADDR_COMM_FUN_CTRL, &u16Data, 1), "ADDR_COMM_FUN_CTRL failed", err_1);
    }
    return true;
err_1:
    return false;
}
bool xp_vd130_global_config(VD130_CTX* const ctx)
{
    uint16_t u16Data;

    CHECK_ERR(ctx, "ctx null", err_1);

    // 命令源选择
    // 0：操作面板命令通道
    // 1：端子命令通道
    // 2：串行口通讯命令通道
    u16Data = (VD130_MODE_COMM == ctx->mode) ? (2) : (1);
    CHECK_ERR(1 == xp_vd130_write_reginster(ctx, ADDR_CMD_SRC, &u16Data, 1), "ADDR_CMD_SRC failed", err_1);

    // 最大频率 50.00～630.00Hz
    u16Data = ctx->maxFreq;
    CHECK_ERR(1 == xp_vd130_write_reginster(ctx, ADDR_MAX_FREQ, &u16Data, 1), "ADDR_MAX_FREQ failed", err_1);

    // 主频率源 A 选择
    // 0：数字设定（UP、DOWN 调节）
    // 1：自带面板电位器
    // 2：外接面板电位器
    // 3：电压输入 VI 给定
    // 4：多段给定
    // 5：PLC
    // 6：PID
    // 7：通讯设定
    // 8：电流输入 CI 给定
    u16Data = (VD130_MODE_COMM == ctx->mode) ? (7) : (4);
    CHECK_ERR(1 == xp_vd130_write_reginster(ctx, ADDR_A_FREQ_SRC, &u16Data, 1), "ADDR_A_FREQ_SRC failed", err_1);

    // 运行方向
    // 0：方向一致
    // 1：方向相反
    u16Data = (ctx->isReverseDir) ? (1) : (0);
    CHECK_ERR(1 == xp_vd130_write_reginster(ctx, ADDR_RUN_DIRECT, &u16Data, 1), "ADDR_RUN_DIRECT failed", err_1);

    // 加速时间 1 0.1s～3600.0s
    u16Data = ctx->maxFreq * 10 / ctx->acceleration;
    CHECK_ERR(1 == xp_vd130_write_reginster(ctx, ADDR_ACC_TIME, &u16Data, 1), "ADDR_ACC_TIME failed", err_1);

    // 减速时间 1 0.1s～3600.0s
    u16Data = ctx->maxFreq * 10 / ctx->deceleration;
    CHECK_ERR(1 == xp_vd130_write_reginster(ctx, ADDR_DEC_TIME, &u16Data, 1), "ADDR_DEC_TIME failed", err_1);

    // 加减速方式
    // 0：直线加减速
    // 1：S 曲线加减速
    u16Data = 1;
    CHECK_ERR(1 == xp_vd130_write_reginster(ctx, ADDR_ACC_TYPE, &u16Data, 1), "ADDR_ACC_TYPE failed", err_1);

    // 停机方式
    // 0：减速停机
    // 1：自由停机
    u16Data = 0;
    CHECK_ERR(1 == xp_vd130_write_reginster(ctx, ADDR_BRAKE_TYPE, &u16Data, 1), "ADDR_BRAKE_TYPE failed", err_1);

    // 停机直流制动起始频率 0.00Hz～最大频率（P0-02）
    u16Data = 20;
    CHECK_ERR(1 == xp_vd130_write_reginster(ctx, ADDR_BRAKE_FREQ, &u16Data, 1), "ADDR_BRAKE_FREQ failed", err_1);

    // 停机直流制动电流 0%～100%
    u16Data = 30;
    CHECK_ERR(1 == xp_vd130_write_reginster(ctx, ADDR_BRAKE_CURR, &u16Data, 1), "ADDR_BRAKE_CURR failed", err_1);

    // 停机直流制动时间 0.0s～500.0s
    u16Data = 20;
    CHECK_ERR(1 == xp_vd130_write_reginster(ctx, ADDR_BRAKE_TIME, &u16Data, 1), "ADDR_BRAKE_TIME failed", err_1);

    // DIx 端子功能选择
    // 0：无功能
    // 1：正转运行（FWD）
    // 2：反转运行（REV）
    // 3：三线式运行控制（Sin）
    // 4：正转点动（FJOG）
    // 5：反转点动（RJOG）
    // 6：端子 UP
    // 7：端子 DOWN
    // 8：自由停车
    // 9：故障复位（RESET）
    // 10：运行暂停
    // 11：备用
    // 12：外部故障输入（常开）
    // 13：多段速端子 1
    // 14：多段速端子 2
    // 15：多段速端子 3
    // 16：多段速端子 4
    // 17：加减速时间选择端子
    // 18：UP/DOWN 设定清零
    // 19：备用
    // 20：加减速禁止
    // 21：PID 暂停
    // 22：PLC 状态复位
    // 23：摆频暂停
    // 24：计数器输入
    // 25：计数器复位
    // 26：长度计数输入
    // 27：长度计数复位
    // 28：备用
    // 29：频率源切换为 AI
    // 30：备用
    // 31：键盘命令源切换
    // 32：端子命令源切换
    // 33：备用
    // 34：频率源切换
    // 35：锁定运行频率
    // 36：选择主频率源
    // 37：选择辅助频率源
    // 38：备用
    // 39：AI 输入对应 AI2 范围
    for (size_t i = 0; i < sizeof(ctx->DIx) / sizeof(ctx->DIx[0]); i++) {
        u16Data = ctx->DIx[i].func;
        CHECK_ERR(1 == xp_vd130_write_reginster(ctx, ADDR_DI1_FUN + i, &u16Data, 1), "ADDR_DIx_FUN failed", err_1);
    }
    // 端子命令方式
    // 0：两线式 1
    // 1：两线式 2
    // 2：三线式 1
    // 3：三线式 2
    u16Data = 0;
    CHECK_ERR(1 == xp_vd130_write_reginster(ctx, ADDR_PORT_CMD_MODE, &u16Data, 1), "ADDR_PORT_CMD_MODE failed", err_1);

    // AO 输出选择
    // 0：运行频率
    // 1：设定频率
    // 2：输出电流
    // 3：输出电压
    // 4：AI
    // 5：长度值
    // 6：计数值
    // 7：通讯控制输出
    u16Data = 2;
    CHECK_ERR(1 == xp_vd130_write_reginster(ctx, ADDR_AO_SEL, &u16Data, 1), "ADDR_AO_SEL failed", err_1);

    // 电机额定电流 0.1A ~ 6553.5A
    u16Data = ctx->motorRatedCurr * 10;
    CHECK_ERR(1 == xp_vd130_write_reginster(ctx, ADDR_M_RATED_CURR, &u16Data, 1), "ADDR_M_RATED_CURR failed", err_1);

    // 电机额定电压 0V ~ 480V
    u16Data = ctx->motorRatedVol;
    CHECK_ERR(1 == xp_vd130_write_reginster(ctx, ADDR_M_RATED_VOL, &u16Data, 1), "ADDR_M_RATED_VOL failed", err_1);

    // 电机额定频率 0.01Hz ~ 最大频率（P0-01）
    u16Data = ctx->motorRatedFreq * 100;
    CHECK_ERR(1 == xp_vd130_write_reginster(ctx, ADDR_M_RATED_FREQ, &u16Data, 1), "ADDR_M_RATED_FREQ failed", err_1);

    // 电机额定功率 0.01KW ~ 300KW
    u16Data = ctx->motorRatedPower / 10;
    CHECK_ERR(1 == xp_vd130_write_reginster(ctx, ADDR_M_RATED_POWER, &u16Data, 1), "ADDR_M_RATED_POWER failed", err_1);

    // 电机额定转速 0 ~ 10000rpm
    u16Data = ctx->motorRatedSpeed;
    CHECK_ERR(1 == xp_vd130_write_reginster(ctx, ADDR_M_RATED_SPEED, &u16Data, 1), "ADDR_M_RATED_SPEED failed", err_1);

    // 多段速 -100.0% ~ 100.0%
    CHECK_ERR(xp_vd130_stageSpeed_config(ctx, ctx->stageSpeedList, sizeof(ctx->stageSpeedList) / sizeof(ctx->stageSpeedList[0])), "xp_vd130_stageSpeed_config failed", err_1);

    // 通讯超时时间 0.1 ~ 60.0s
    u16Data = (VD130_MODE_COMM == ctx->mode) ? (ctx->commOvertime_ms / 100) : (0);
    CHECK_ERR(1 == xp_vd130_write_reginster(ctx, ADDR_COMM_OVER_TIME, &u16Data, 1), "ADDR_COMM_OVER_TIME failed", err_1);

    return true;
err_1:
    return false;
}
/**************************************内部接口**********************************************/
/**
 * @brief       VD130 切换到端子运行模式(变频器需已出厂配置)
 *              此接口仅切换命令通道为端子,频率源为多段速
 * @param[in]	ctx                 接口handle
 * @return      bool                true: 切换成功
 */
static bool xp_vd130_switch_to_port_mode(VD130_CTX* const ctx)
{
    uint16_t u16Data;

    u16Data = 1;
    CHECK_ERR(1 == xp_vd130_write_reginster(ctx, ADDR_CMD_SRC, &u16Data, 1), "ADDR_CMD_SRC failed", err_1);

    u16Data = 4;
    CHECK_ERR(1 == xp_vd130_write_reginster(ctx, ADDR_A_FREQ_SRC, &u16Data, 1), "ADDR_A_FREQ_SRC failed", err_1);

    u16Data = 0;
    CHECK_ERR(1 == xp_vd130_write_reginster(ctx, ADDR_COMM_OVER_TIME, &u16Data, 1), "ADDR_COMM_OVER_TIME failed", err_1);

    ctx->mode = VD130_MODE_PORT;
    return true;
err_1:
    return false;
}
/**
 * @brief       VD130 切换到通信运行模式(变频器需已出厂配置)
 *              此接口仅切换命令通道为通信,频率源为通信给定
 *              通信模式下出于安全考虑,配置通信超时时间,超时后停机
 * @param[in]	ctx                 接口handle
 * @return      bool                true: 切换成功
 */
static bool xp_vd130_switch_to_comm_mode(VD130_CTX* const ctx)
{
    uint16_t u16Data;

    u16Data = 2;
    CHECK_ERR(1 == xp_vd130_write_reginster(ctx, ADDR_CMD_SRC, &u16Data, 1), "ADDR_CMD_SRC failed", err_1);

    u16Data = 7;
    CHECK_ERR(1 == xp_vd130_write_reginster(ctx, ADDR_A_FREQ_SRC, &u16Data, 1), "ADDR_A_FREQ_SRC failed", err_1);

    u16Data = ctx->commOvertime_ms / 100;
    CHECK_ERR(1 == xp_vd130_write_reginster(ctx, ADDR_COMM_OVER_TIME, &u16Data, 1), "ADDR_COMM_OVER_TIME failed", err_1);

    ctx->mode = VD130_MODE_COMM;
    return true;
err_1:
    return false;
}


/**
 * @brief       控制 IO 接口
 * @param[in]	board_id            板ID
 * @param[in]	pin                 pin ID
 * @param[in]	isEnable            是否使能
 */
static void xp_vd130_write_pin(Type_GPIO_Def* const pIo, const bool isEnable)
{
    if (pIo->boardId > 0) {    //通过板ID是否有效判断此IO是否实际使用
        xp_io_write_pin(pIo->boardId, pIo->pinId, isEnable ? (0) : (1));
    }
}
/**
 * @brief       Modbus 写寄存器
 * @param[in]	ctx                 接口handle
 * @param[in]	regAddr             寄存器地址
 * @param[in]	pData               寄存器数据指针
 * @param[in]	regNum              寄存器数量(单位uint16_t)
 * @return      uint8_t             写入寄存器数量
 */
static uint8_t xp_vd130_write_reginster(VD130_CTX* const ctx, uint16_t regAddr, uint16_t* pData, uint8_t regNum)
{
    uint8_t resLen;
    MB_data modbus = {
        .slave_id = ctx->slaveId,
        .mb_fun = mb_single_w,
        .start_addr = regAddr,
        .wBuf = pData,
        .rwLen = regNum,
        .resBuf = (uint8_t*) pData,
        .resLen = &resLen
    };
    if (regNum > 1) {
        modbus.mb_fun = mb_hold_w;
    }
    aos_mutex_lock(ctx->mutex, AOS_WAIT_FOREVER);
    int ret = xp_modbus_ask(ctx->port, &modbus);
    aos_mutex_unlock(ctx->mutex);
    if (0 == ret) {
        ctx->lostCnts = 0;
        return resLen / 2;
    }
    if (ctx->lostCnts < UINT8_MAX) {
        ctx->lostCnts++;
    }
    return 0;
}
/**
 * @brief       Modbus 读寄存器
 * @param[in]	ctx                 接口handle
 * @param[in]	regAddr             寄存器地址
 * @param[in]	pData               寄存器数据指针
 * @param[in]	regNum              寄存器数量(单位uint16_t)
 * @return      uint8_t             读取寄存器数量
 */
static uint8_t xp_vd130_read_reginster(VD130_CTX* const ctx, uint16_t regAddr, uint16_t* pData, uint8_t regNum)
{
    uint8_t resLen;
    MB_data modbus = {
        .slave_id = ctx->slaveId,
        .mb_fun = mb_hold_r,
        .start_addr = regAddr,
        .wBuf = pData,
        .rwLen = regNum,
        .resBuf = (uint8_t*) pData,
        .resLen = &resLen
    };

    aos_mutex_lock(ctx->mutex, AOS_WAIT_FOREVER);
    int ret = xp_modbus_ask(ctx->port, &modbus);
    aos_mutex_unlock(ctx->mutex);
    if (0 == ret) {
        ctx->lostCnts = 0;
        for (size_t i = 0; i < resLen / 2; i++) {
            uint16_t tmp = (pData[i] >> 8) & 0xFF;
            pData[i] = (pData[i] << 8) | tmp;
            // pData[i] = __REV16(pData[i]);   // 翻转字节序
        }
        return resLen / 2;
    }
    if (ctx->lostCnts < UINT8_MAX) {
        ctx->lostCnts++;
    }
    return 0;
}

static void vd130_polling_thread(void* arg)
{
    VD130_CTX* ctx = (VD130_CTX*) arg;
    uint64_t timeStamp = aos_now_ms();
    uint16_t u16Data;

    while (1) {
        /*******************************************************/
        if (ctx->isReadCurrent) {
            if (xp_vd130_get_current(ctx, NULL)) {
                ctx->event_cb(ctx, VD130_EVENT_CURRENT, u16Data);
            }
        }
        // 定时读取故障状态
        /*******************************************************/
        if (aos_now_ms() - timeStamp > FAULT_LOOP_TIME_MS) {
            timeStamp = aos_now_ms();
            if (xp_vd130_get_fault_code(ctx, &u16Data)) {
                ctx->event_cb(ctx, VD130_EVENT_FAULT, u16Data);
            }
        }
        // 在连接状态改变时回调事件
        /*******************************************************/
        if (ctx->lostCnts > VD130_OFFLINE_THRESHOLD) {
            if (ctx->isOnline) {
                ctx->isOnline = false;
                ctx->event_cb(ctx, VD130_EVENT_CONNECT, 0);
            }
        }
        else {
            if (!ctx->isOnline) {
                ctx->isOnline = true;
                ctx->event_cb(ctx, VD130_EVENT_CONNECT, 1);
            }
        }
        /*******************************************************/
        aos_msleep(ctx->interval_ms);
    }
}

#if 0
static VD130_CTX* pDemo_motorDrvs[2] = { NULL };

static void event_callback_templete(VD130_CTX* ctx, VD130_EVENT_enmu eventId, uint32_t data)
{
    switch (eventId) {
    case VD130_EVENT_CONNECT:
        // 掉线/上线
        println("VD130 id: %d, %s", xp_vd130_get_id(ctx), data ? "online" : "offline");
        break;
    case VD130_EVENT_CURRENT:
        // 值电流读取事件 
        println("VD130 id: %d, Current: %dmA", xp_vd130_get_id(ctx), (int32_t) data);
        break;
    case VD130_EVENT_FAULT:
        // 故障代码读取事件 
        if (data) {
            println("VD130 id: %d, FaultCode: %d", xp_vd130_get_id(ctx), data);
            xp_vd130_clear_fault(ctx);
            // app_fault_report(data);
        }
        break;
    default:
        break;
    }
}

bool demo_app_init(void)
{
    bool ret = false;
    pDemo_motorDrvs[0] = xp_vd130_init(1, 1, 9600, 0, event_callback_templete);
    if (pDemo_motorDrvs[0]) {
        /****************以下参数仅需要出厂时配置一次即可****************/
        pDemo_motorDrvs[0]->mode = VD130_MODE_COMM;     // 配置为通信给定模式
        pDemo_motorDrvs[0]->isReverseDir = true;        // 配置反转方向
        pDemo_motorDrvs[0]->acceleration = 10;          // 配置加速度 10Hz/s
        pDemo_motorDrvs[0]->deceleration = 20;          // 配置减速度 20HZ/s
        pDemo_motorDrvs[0]->commOvertime_ms = 3000;     // 配置通信超时时间 3s
        pDemo_motorDrvs[0]->maxFreq = 50;               // 配置最大频率 50Hz(必须)
        pDemo_motorDrvs[0]->motorRatedCurr = 1.2;       // 配置电机额定电流 1.2A
        pDemo_motorDrvs[0]->motorRatedFreq = 50;        // 配置电机额定频率 50Hz
        pDemo_motorDrvs[0]->motorRatedPower = 500;      // 配置电机额定功率 500W
        pDemo_motorDrvs[0]->motorRatedSpeed = 1130;     // 配置电机额定转速 1130rpm
        pDemo_motorDrvs[0]->motorRatedVol = 220;        // 配置电机额定电压 220V
        ret = xp_vd130_global_config(pDemo_motorDrvs[0]);
        /**************************************************************/
    }
    else {
        return false;
    }

    pDemo_motorDrvs[1] = xp_vd130_init(2, 1, 9600, 0, NULL);
    if (pDemo_motorDrvs[1] && ret) {
        /****************以下参数仅需要出厂时配置一次即可****************/
        pDemo_motorDrvs[1]->mode = VD130_MODE_PORT;     // 配置为端子模式
        pDemo_motorDrvs[1]->isReverseDir = false;       // 配置不反转方向
        pDemo_motorDrvs[1]->acceleration = 10;          // 配置加速度 10Hz/s
        pDemo_motorDrvs[1]->deceleration = 20;          // 配置减速度 20HZ/s
        pDemo_motorDrvs[1]->maxFreq = 50;               // 配置最大频率 50Hz(必须)
        pDemo_motorDrvs[1]->motorRatedCurr = 1.2;       // 配置电机额定电流 1.2A
        pDemo_motorDrvs[1]->motorRatedFreq = 50;        // 配置电机额定频率 50Hz
        pDemo_motorDrvs[1]->motorRatedPower = 500;      // 配置电机额定功率 500W
        pDemo_motorDrvs[1]->motorRatedSpeed = 1130;     // 配置电机额定转速 1130rpm
        pDemo_motorDrvs[1]->motorRatedVol = 220;        // 配置电机额定电压 220V

        pDemo_motorDrvs[1]->DIx[0].func = 13;           // 配置为多段速选择 1
        pDemo_motorDrvs[1]->DIx[0].io = (Type_GPIO_Def){ .boardId = 1, .pinId = 1 };
        pDemo_motorDrvs[1]->DIx[1].func = 14;           // 配置为多段速选择 2
        pDemo_motorDrvs[1]->DIx[1].io = (Type_GPIO_Def){ .boardId = 1, .pinId = 2 };
        pDemo_motorDrvs[1]->DIx[2].func = 13;           // 配置为多段速选择 3
        pDemo_motorDrvs[1]->DIx[2].io = (Type_GPIO_Def){ .boardId = 1, .pinId = 3 };
        pDemo_motorDrvs[1]->DIx[3].func = 10;           // 配置为运行暂停
        pDemo_motorDrvs[1]->DIx[3].io = (Type_GPIO_Def){ .boardId = 1, .pinId = 4 };

        ret = xp_vd130_global_config(pDemo_motorDrvs[1]);
        /**************************************************************/
    }
    else {
        ret = xp_vd130_deinit(pDemo_motorDrvs[0]);
    }

    return pDemo_motorDrvs[0] && pDemo_motorDrvs[1] && ret;
}
bool demo_motor_run_stageSpeed(VD130_CTX* const ctx, int8_t speedLevel)
{
#define STAGE_RANGE  3  // speedLevel: (-3 ~ 3)
    static uint8_t stageSpeedMapping_3[] = { 6, 4, 2, 0, 1, 3, 5 };   // 此映射数组需要根据段速范围和段速配置表生成
    if (speedLevel >= -STAGE_RANGE && speedLevel <= STAGE_RANGE) {
        return xp_vd130_run_stageSpeed(pDemo_motorDrvs[1], stageSpeedMapping_3[speedLevel + STAGE_RANGE]);
    }
    return false;
}
void demo_app_task(void)
{
    xp_vd130_set_loop_state(pDemo_motorDrvs[0], rue);   // 配置驱动层轮询读取变频器 0 电流值
    xp_vd130_run_comm(pDemo_motorDrvs[0], 30.5);   // 通信方式设定变频器 0 以 30.5Hz 运行(由轮询线程维护心跳,失联变频器自动停机)

    demo_motor_run_stageSpeed(pDemo_motorDrvs[1], -2);   // 多段速方式设定变频器 1 以第二级别转速反转(-30Hz)
}


#endif